(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[     60244,       1674]
NotebookOptionsPosition[     56996,       1562]
NotebookOutlinePosition[     57388,       1579]
CellTagsIndexPosition[     57345,       1576]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell["2d kneed biped with torso", "Title",
 CellChangeTimes->{
  3.504390813710744*^9, {3.513011450427533*^9, 3.51301145527552*^9}, {
   3.513011745947039*^9, 3.513011749321201*^9}, {3.515858432663828*^9, 
   3.5158584360910234`*^9}, {3.5158700644438047`*^9, 
   3.5158700646998196`*^9}, {3.5158859650463057`*^9, 3.515885965342706*^9}}],

Cell[CellGroupData[{

Cell["initialization", "Section",
 CellChangeTimes->{
  3.504390830432091*^9, {3.513011606627004*^9, 3.513011611923624*^9}, {
   3.513011744096037*^9, 3.513011744363876*^9}, 3.514056528643134*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}], "\n", 
 RowBox[{"<<", "RobotLinks.m"}], "\n", 
 RowBox[{"<<", "Linearize.m"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527682981510345*^9, 3.52768298184157*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"t0", "=", 
   RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5222466960362115`*^9, 3.5222467018365436`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{"A_", "?", "MatrixQ"}], "]"}], ":=", 
  RowBox[{"ParallelTable", "[", 
   RowBox[{
    RowBox[{"Simplify", "[", 
     RowBox[{"A", "[", 
      RowBox[{"[", 
       RowBox[{"i", ",", "j"}], "]"}], "]"}], "]"}], ",", 
    RowBox[{"{", 
     RowBox[{"i", ",", 
      RowBox[{
       RowBox[{"Dimensions", "[", "A", "]"}], "[", 
       RowBox[{"[", "1", "]"}], "]"}]}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{"j", ",", 
      RowBox[{
       RowBox[{"Dimensions", "[", "A", "]"}], "[", 
       RowBox[{"[", "2", "]"}], "]"}]}], "}"}]}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{"A_", "?", "VectorQ"}], "]"}], ":=", 
  RowBox[{"ParallelTable", "[", 
   RowBox[{
    RowBox[{"Simplify", "[", 
     RowBox[{"A", "[", 
      RowBox[{"[", "i", "]"}], "]"}], "]"}], ",", 
    RowBox[{"{", 
     RowBox[{"i", ",", 
      RowBox[{"Length", "[", "A", "]"}]}], "}"}]}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{"ParallelSimplify", "[", "A_", "]"}], ":=", 
  RowBox[{"Simplify", "[", "A", "]"}]}]}], "Input",
 CellChangeTimes->{{3.5276785593602753`*^9, 3.527678559370429*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["model specification", "Section",
 CellChangeTimes->{{3.513531803717296*^9, 3.513531806022168*^9}, {
  3.51767999292592*^9, 3.5176799960260973`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"Needs", "[", "\"\<Units`\>\"", "]"}], "\n", 
 RowBox[{"robotInfo", "=", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{
     "Lc", ",", "Lt", ",", "LT", ",", "Lb", ",", "rc", ",", "rt", ",", "rT", 
      ",", "mh", ",", "mt", ",", "mc", ",", "mf", ",", "g", ",", "mxx", ",", 
      "mzz"}], "}"}], ",", "\[IndentingNewLine]", 
    RowBox[{"{", 
     RowBox[{"347.98", ",", "261.112", ",", "0", ",", 
      RowBox[{
       FractionBox[
        RowBox[{"MKS", "[", 
         RowBox[{"2.13", " ", "Inch"}], "]"}], "Meter"], "*", "1000"}], ",", 
      "282.37", ",", "128.15", ",", "9.97", ",", 
      StyleBox["804.83",
       FontWeight->"Plain"], ",", "606.15", ",", "213.79", ",", "0", ",", 
      "9810", ",", "2490", ",", "880"}], "}"}]}], "}"}]}], "\n", 
 RowBox[{"constsubs", "=", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{
     RowBox[{
      RowBox[{"#1", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
      "\[Rule]", 
      RowBox[{"Rationalize", "[", 
       FractionBox[
        RowBox[{"#1", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
        "1000"], "]"}]}], "&"}], ")"}], "/@", 
   RowBox[{"Transpose", "[", "robotInfo", "]"}]}]}], "\n", 
 RowBox[{"{", 
  RowBox[{
   RowBox[{"Lc", "\[Rule]", 
    FractionBox["17399", "50000"]}], ",", 
   RowBox[{"Lt", "\[Rule]", 
    FractionBox["32639", "125000"]}], ",", 
   RowBox[{"LT", "\[Rule]", "0"}], ",", 
   RowBox[{"Lb", "\[Rule]", 
    FractionBox["27051", "500000"]}], ",", 
   RowBox[{"rc", "\[Rule]", 
    FractionBox["28237", "100000"]}], ",", 
   RowBox[{"rt", "\[Rule]", 
    FractionBox["2563", "20000"]}], ",", 
   RowBox[{"rT", "\[Rule]", 
    FractionBox["997", "100000"]}], ",", 
   RowBox[{"mh", "\[Rule]", 
    FractionBox["80483", "100000"]}], ",", 
   RowBox[{"mt", "\[Rule]", 
    FractionBox["12123", "20000"]}], ",", 
   RowBox[{"mc", "\[Rule]", 
    FractionBox["21379", "100000"]}], ",", 
   RowBox[{"mf", "\[Rule]", "0"}], ",", 
   RowBox[{"g", "\[Rule]", 
    FractionBox["981", "100"]}], ",", 
   RowBox[{"mxx", "\[Rule]", 
    FractionBox["249", "100"]}], ",", 
   RowBox[{"mzz", "\[Rule]", 
    FractionBox["22", "25"]}]}], "}"}], "\n", 
 RowBox[{
  RowBox[{"ndof", " ", "=", " ", "5"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"mm", "=", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"statesubs", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", "i", "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "i"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", 
         RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", "12.37"},
         {"0", "1946798.09", 
          RowBox[{"-", "19.96"}]},
         {"12.37", 
          RowBox[{"-", "19.96"}], "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", "58.80", "2024.23"},
         {"58.80", "6396011.40", "146697.91"},
         {"2024.23", "146697.91", "418371.07"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"3730232.81", 
          RowBox[{"-", "7.44"}], "1948.96"},
         {
          RowBox[{"-", "7.44"}], "518271.83", 
          RowBox[{"-", "1348.28"}]},
         {"1948.96", 
          RowBox[{"-", "1348.28"}], "3577190.08"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", 
          RowBox[{"-", "60.11"}], "1985.63"},
         {
          RowBox[{"-", "60.11"}], "6396011.38", 
          RowBox[{"-", "146697.91"}]},
         {"1985.63", 
          RowBox[{"-", "146697.91"}], "418371.06"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", 
          RowBox[{"-", "12.37"}]},
         {"0", "1946798.091", "19.96"},
         {
          RowBox[{"-", "12.37"}], "19.96", "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", 
    RowBox[{"Length", "[", "mm", "]"}]}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
         RowBox[{"IdentityMatrix", "[", "3", "]"}]}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", 
        SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", "1"}],
      "]"}]}]}], "]"}], "\n", 
 RowBox[{
  RowBox[{"p0", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SuperscriptBox[
        SubscriptBox["p", "x"], "\[Prime]",
        MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SuperscriptBox[
        SubscriptBox["p", "z"], "\[Prime]",
        MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}]}], 
    "}"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5276784158568068`*^9, {3.5276784733021917`*^9, 3.527678494938101*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"Lc", "Lt", "LT", "Lb", "rc", "rt", "rT", "mh", "mt", "mc", "mf", "g", 
      "mxx", "mzz"},
     {"347.98`", "261.112`", "0", "54.102`", "282.37`", "128.15`", "9.97`", 
      "804.83`", "606.15`", "213.79`", "0", "9810", "2490", "880"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.527678481212152*^9, 3.527678497399748*^9}, 
   3.5276795445532427`*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{"Lc", "\[Rule]", 
     FractionBox["17399", "50000"]}], ",", 
    RowBox[{"Lt", "\[Rule]", 
     FractionBox["32639", "125000"]}], ",", 
    RowBox[{"LT", "\[Rule]", "0"}], ",", 
    RowBox[{"Lb", "\[Rule]", 
     FractionBox["27051", "500000"]}], ",", 
    RowBox[{"rc", "\[Rule]", 
     FractionBox["28237", "100000"]}], ",", 
    RowBox[{"rt", "\[Rule]", 
     FractionBox["2563", "20000"]}], ",", 
    RowBox[{"rT", "\[Rule]", 
     FractionBox["997", "100000"]}], ",", 
    RowBox[{"mh", "\[Rule]", 
     FractionBox["80483", "100000"]}], ",", 
    RowBox[{"mt", "\[Rule]", 
     FractionBox["12123", "20000"]}], ",", 
    RowBox[{"mc", "\[Rule]", 
     FractionBox["21379", "100000"]}], ",", 
    RowBox[{"mf", "\[Rule]", "0"}], ",", 
    RowBox[{"g", "\[Rule]", 
     FractionBox["981", "100"]}], ",", 
    RowBox[{"mxx", "\[Rule]", 
     FractionBox["249", "100"]}], ",", 
    RowBox[{"mzz", "\[Rule]", 
     FractionBox["22", "25"]}]}], "}"}], TraditionalForm]], "Output",
 CellChangeTimes->{{3.527678481212152*^9, 3.527678497399748*^9}, 
   3.52767954455525*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{"Lc", "\[Rule]", 
     FractionBox["17399", "50000"]}], ",", 
    RowBox[{"Lt", "\[Rule]", 
     FractionBox["32639", "125000"]}], ",", 
    RowBox[{"LT", "\[Rule]", "0"}], ",", 
    RowBox[{"Lb", "\[Rule]", 
     FractionBox["27051", "500000"]}], ",", 
    RowBox[{"rc", "\[Rule]", 
     FractionBox["28237", "100000"]}], ",", 
    RowBox[{"rt", "\[Rule]", 
     FractionBox["2563", "20000"]}], ",", 
    RowBox[{"rT", "\[Rule]", 
     FractionBox["997", "100000"]}], ",", 
    RowBox[{"mh", "\[Rule]", 
     FractionBox["80483", "100000"]}], ",", 
    RowBox[{"mt", "\[Rule]", 
     FractionBox["12123", "20000"]}], ",", 
    RowBox[{"mc", "\[Rule]", 
     FractionBox["21379", "100000"]}], ",", 
    RowBox[{"mf", "\[Rule]", "0"}], ",", 
    RowBox[{"g", "\[Rule]", 
     FractionBox["981", "100"]}], ",", 
    RowBox[{"mxx", "\[Rule]", 
     FractionBox["249", "100"]}], ",", 
    RowBox[{"mzz", "\[Rule]", 
     FractionBox["22", "25"]}]}], "}"}], TraditionalForm]], "Output",
 CellChangeTimes->{{3.527678481212152*^9, 3.527678497399748*^9}, 
   3.527679544557906*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["generalized coordinates", "Subsection",
 CellChangeTimes->{
  3.504391119483818*^9, {3.513012126441401*^9, 3.51301212962473*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"q", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"dq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ddq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"qe", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", "q"}], 
    "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"dqe", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391169851249*^9, 3.504391191970865*^9}, {
   3.5129535326917257`*^9, 3.5129535355376797`*^9}, 3.513011937181409*^9, {
   3.513012138013254*^9, 3.513012139016991*^9}, 3.515858456069166*^9, {
   3.5158695182095623`*^9, 3.515869519316625*^9}, 3.5273331283432217`*^9}],

Cell[CellGroupData[{

Cell["location and direction of twists", "Subsubsection",
 CellChangeTimes->{
  3.504391222051317*^9, {3.513012004436311*^9, 3.513012017657574*^9}}],

Cell["positive z direction, ", "Text",
 CellChangeTimes->{{3.504895806007092*^9, 3.504895813878101*^9}, {
  3.504899673358274*^9, 3.504899680392001*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "0"], "=", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "px"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "pz"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q1"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q2"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q3"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q4"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q5"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391239055382*^9, 3.504391280546184*^9}, {
   3.504393690323417*^9, 3.504393694642943*^9}, {3.504394354402188*^9, 
   3.504394382731842*^9}, 3.504394560552597*^9, {3.504733962343795*^9, 
   3.50473399215855*^9}, {3.504734047207357*^9, 3.504734112723113*^9}, {
   3.504895790317638*^9, 3.504895793671604*^9}, {3.504895835178937*^9, 
   3.504895837468827*^9}, {3.50489969948502*^9, 3.504899711184298*^9}, {
   3.504901355868927*^9, 3.504901367126576*^9}, {3.504901644110264*^9, 
   3.504901651461383*^9}, {3.5125072468769054`*^9, 3.5125072492793093`*^9}, {
   3.512510523268261*^9, 3.512510626680843*^9}, {3.512510703620178*^9, 
   3.5125107228706117`*^9}, {3.5125107694990935`*^9, 
   3.5125107706690955`*^9}, {3.512510874416078*^9, 3.5125108760228806`*^9}, {
   3.5125109282169724`*^9, 3.5125109524594145`*^9}, 3.512511205857059*^9, {
   3.5125116217689886`*^9, 3.5125116245925937`*^9}, {3.512512838505127*^9, 
   3.5125128440899363`*^9}, 3.5125154382821627`*^9, 3.512515631401702*^9, {
   3.512515675315779*^9, 3.512515677234583*^9}, {3.512515713489046*^9, 
   3.51251571545465*^9}, {3.5125157856379766`*^9, 3.5125157895691843`*^9}, {
   3.5125160948139267`*^9, 3.512516101631139*^9}, {3.5125163059110966`*^9, 
   3.512516337906753*^9}, 3.5125169799917154`*^9, 3.512517350494175*^9, {
   3.5125175771075783`*^9, 3.512517615811246*^9}, {3.5125182182931213`*^9, 
   3.5125182275127373`*^9}, {3.513011948790205*^9, 3.513011986836761*^9}, {
   3.513012028383106*^9, 3.513012030110642*^9}, {3.513531858718806*^9, 
   3.513531893067239*^9}, {3.513869388669607*^9, 3.513869391129299*^9}, {
   3.513956274948754*^9, 3.513956281701611*^9}, {3.5155018106921597`*^9, 
   3.515501813883342*^9}, {3.515501850954462*^9, 3.5155018542846527`*^9}, {
   3.5158584657537203`*^9, 3.515858513238436*^9}, {3.515869263882015*^9, 
   3.5158692653601*^9}, {3.5158703231956043`*^9, 3.515870326599799*^9}, {
   3.515886006277178*^9, 3.5158860120335884`*^9}, {3.5167081937628703`*^9, 
   3.5167082227325273`*^9}, 3.51670884415207*^9, {3.52742382151624*^9, 
   3.527423824502006*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["base configuration", "Subsubsection",
 CellChangeTimes->{
  3.504391314503995*^9, {3.513011998415469*^9, 3.513011998449444*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         FractionBox[
          RowBox[{"-", "33"}], "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{
         FractionBox["2", "100000"], ",", 
         FractionBox[
          RowBox[{"-", "381"}], "100000"], ",", 
         RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{
         FractionBox[
          RowBox[{"-", "24"}], "100000"], ",", 
         FractionBox[
          RowBox[{"-", "4"}], "100000"], ",", 
         RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{
         FractionBox["2", "100000"], ",", 
         FractionBox["381", "100000"], ",", 
         RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         FractionBox["33", "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.5158497160803623`*^9, 3.5158497388296633`*^9}, {3.5158505076696386`*^9, 
   3.5158505174832*^9}, {3.51585069440932*^9, 3.5158507070580435`*^9}, {
   3.5158510492876177`*^9, 3.5158510493766227`*^9}, {3.5158515449339666`*^9, 
   3.515851552726413*^9}, {3.5158516115196805`*^9, 3.5158516208742156`*^9}, {
   3.515852304074292*^9, 3.515852316172984*^9}, {3.5158524561829925`*^9, 
   3.515852474285028*^9}, {3.515859753607381*^9, 3.5158597547104445`*^9}, {
   3.515869286854329*^9, 3.5158692891464605`*^9}, {3.5158703778677316`*^9, 
   3.515870383570058*^9}, {3.5158860185856*^9, 3.5158860205200033`*^9}, {
   3.516707969937068*^9, 3.5167079805786767`*^9}, {3.5167082033634195`*^9, 
   3.516708212488941*^9}, 3.516708841559922*^9, {3.5167096385125055`*^9, 
   3.5167096393665543`*^9}, {3.517679210208151*^9, 3.5176792179195924`*^9}, {
   3.5176856517675877`*^9, 3.5176856652333574`*^9}, {3.5176858174780655`*^9, 
   3.5176858538481455`*^9}, {3.5212226605105443`*^9, 
   3.5212226803056765`*^9}, {3.5212227191418977`*^9, 3.52122290328043*^9}, {
   3.521292043444914*^9, 3.5212920980150347`*^9}, {3.521292133845084*^9, 
   3.5212921713812313`*^9}, {3.5212922405391865`*^9, 3.521292296637395*^9}, {
   3.5214850234198947`*^9, 3.521485050627451*^9}, {3.5222465278455915`*^9, 
   3.5222465426854405`*^9}, {3.5245004568416204`*^9, 
   3.5245004677162423`*^9}, {3.5245014065889425`*^9, 
   3.5245014068999605`*^9}, {3.524501486108491*^9, 3.5245014880206003`*^9}, {
   3.5245022041465607`*^9, 3.524502211992009*^9}, {3.524510625029207*^9, 
   3.5245106719988937`*^9}, 3.52509193018268*^9, {3.527422810039141*^9, 
   3.527422825573169*^9}, {3.52742382507415*^9, 3.527423826555172*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Table", "[", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "j"], "=", 
     RowBox[{"ParallelSimplify", "[", 
      RowBox[{"BodyJacobian", "@@", 
       RowBox[{"Append", "[", 
        RowBox[{
         RowBox[{"Join", "[", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{
             RowBox[{"{", 
              RowBox[{
               SubscriptBox["\[Xi]", "px"], ",", 
               RowBox[{
                SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
             RowBox[{"{", 
              RowBox[{
               SubscriptBox["\[Xi]", "pz"], ",", 
               RowBox[{
                SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}]}], "}"}], 
           ",", 
           RowBox[{"Table", "[", 
            RowBox[{
             RowBox[{"{", 
              RowBox[{
               SubscriptBox["\[Xi]", 
                RowBox[{"If", "[", 
                 RowBox[{
                  RowBox[{"i", "\[LessEqual]", "j"}], ",", 
                  RowBox[{"Symbol", "[", 
                   RowBox[{"\"\<q\>\"", "<>", 
                    RowBox[{"ToString", "[", "i", "]"}]}], "]"}], ",", "0"}], 
                 "]"}]], ",", 
               RowBox[{
                SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "}"}], ",", 
             
             RowBox[{"{", 
              RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}], 
         ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "j"]], "[", "0", "]"}]}], "]"}]}], "]"}]}], ",", 
    RowBox[{"{", 
     RowBox[{"j", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ";"}]], "Input",
 CellChangeTimes->{{3.527422844507322*^9, 3.527423234035161*^9}, {
  3.527423266132931*^9, 3.527423277907259*^9}, {3.527423545447129*^9, 
  3.527423773742435*^9}, {3.527423920930048*^9, 3.527423983161631*^9}, {
  3.527424018646358*^9, 3.527424019089961*^9}, {3.527424065466184*^9, 
  3.527424070964226*^9}, {3.52742410500295*^9, 3.527424105766491*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Table", "[", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "j"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"ParallelSimplify", "@", 
      RowBox[{"(", 
       RowBox[{"ForwardKinematics", "@@", 
        RowBox[{"Append", "[", 
         RowBox[{
          RowBox[{"Table", "[", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{
              SubscriptBox["\[Xi]", 
               RowBox[{"If", "[", 
                RowBox[{
                 RowBox[{"i", "\[LessEqual]", "j"}], ",", 
                 RowBox[{"Symbol", "[", 
                  RowBox[{"\"\<q\>\"", "<>", 
                   RowBox[{"ToString", "[", "i", "]"}]}], "]"}], ",", "0"}], 
                "]"}]], ",", 
              RowBox[{
               SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "}"}], ",", 
            RowBox[{"{", 
             RowBox[{"i", ",", 
              RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}], ",", 
          RowBox[{
           SubscriptBox["g", 
            SubscriptBox["sl", "j"]], "[", "0", "]"}]}], "]"}]}], ")"}]}]}], 
    ",", 
    RowBox[{"{", 
     RowBox[{"j", ",", 
      RowBox[{"Length", "[", "mm", "]"}]}], "}"}]}], "]"}], ";"}]], "Input",
 CellChangeTimes->{{3.5222457074086657`*^9, 3.5222458194940767`*^9}, {
   3.5222458544370747`*^9, 3.522245923830044*^9}, {3.5222459999894*^9, 
   3.5222462016349335`*^9}, {3.52224625157579*^9, 3.5222463954800205`*^9}, {
   3.5222464455368843`*^9, 3.522246520224156*^9}, {3.5222465554811726`*^9, 
   3.5222466059050565`*^9}, {3.5222466519056873`*^9, 3.522246673520924*^9}, 
   3.525446547157471*^9, 3.527423831330376*^9, {3.52742411130584*^9, 
   3.527424133822552*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the forward kinematics maps", "Subsubsection",
 CellChangeTimes->{
  3.504391365377686*^9, {3.513012058700656*^9, 3.513012059089004*^9}, {
   3.513013029501931*^9, 3.513013029568491*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";", 
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.513875671984485*^9, 3.513875737139051*^9}, {3.515858621125607*^9, 
   3.51585862956809*^9}, {3.5158693201992364`*^9, 3.515869321718323*^9}, {
   3.515886061969276*^9, 3.5158860734820957`*^9}, {3.516708239115464*^9, 
   3.5167082431056924`*^9}, {3.517679572905896*^9, 3.5176795767731175`*^9}, {
   3.5245015787767916`*^9, 3.5245015877623053`*^9}, {3.527424174451967*^9, 
   3.527424174975839*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "]"}]}], ";", 
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.513875795389027*^9, 3.513875935660706*^9}, 3.513876139346497*^9, {
   3.5158497919627028`*^9, 3.515849794085824*^9}, {3.515858606539773*^9, 
   3.5158586531084366`*^9}, {3.515869325403534*^9, 3.515869328852731*^9}, {
   3.5158695423689437`*^9, 3.515869545677133*^9}, {3.5158697121476545`*^9, 
   3.5158697210631647`*^9}, {3.5158702933078947`*^9, 
   3.5158703090517955`*^9}, {3.5158860863521185`*^9, 3.515886097818139*^9}, {
   3.515886139454612*^9, 3.5158861478630266`*^9}, {3.516708246161867*^9, 
   3.5167082520592046`*^9}, {3.5245015934336295`*^9, 
   3.5245016073424253`*^9}, {3.527423835292002*^9, 3.527423837750752*^9}, {
   3.527424185288641*^9, 3.527424209779361*^9}}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Choosing the Outputs", "Section",
 CellChangeTimes->{{3.5206963594857893`*^9, 3.520696361509555*^9}, {
  3.5210630974288096`*^9, 3.521063101088019*^9}}],

Cell["calculate COM for later use", "Text",
 CellChangeTimes->{{3.527517849280295*^9, 3.527517851164325*^9}, {
  3.527518048751359*^9, 3.52751805237524*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["p", "COM"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    FractionBox[
     RowBox[{
      UnderoverscriptBox["\[Sum]", 
       RowBox[{"i", "=", "1"}], 
       RowBox[{"Length", "[", "mm", "]"}]], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
        "\[RightDoubleBracket]"}], " ", 
       RowBox[{
       "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}]}]}], 
     RowBox[{"Total", "[", "mm", "]"}]], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.527517851934319*^9, 3.527517873584816*^9}, {
   3.527517942241016*^9, 3.527518031085544*^9}, {3.527518364898716*^9, 
   3.527518402736513*^9}, 3.527540516409532*^9, {3.527678737785783*^9, 
   3.527678760844458*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"TorsoAngle", "=", 
   RowBox[{
    UnderoverscriptBox["\[Sum]", 
     RowBox[{"i", "=", "1"}], "3"], 
    RowBox[{
     SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}]}], ";"}]], "Input",
 CellChangeTimes->{{3.52069555679044*^9, 3.520695570667728*^9}, {
  3.520709652691082*^9, 3.520709653905396*^9}, {3.527518107587208*^9, 
  3.527518153503807*^9}, {3.527518190539578*^9, 3.527518190946951*^9}, {
  3.52751921934838*^9, 3.527519237377073*^9}, {3.5276789948711557`*^9, 
  3.5276790041580477`*^9}, {3.527679126031966*^9, 3.527679126402411*^9}}],

Cell[CellGroupData[{

Cell["stance-COM slope", "Subsubsection",
 CellChangeTimes->{{3.527518321277124*^9, 3.527518327763413*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"slopeStCOM", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        SubscriptBox["p", "COM"], "\[LeftDoubleBracket]", "1", 
        "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        SubscriptBox["p", "COM"], "\[LeftDoubleBracket]", "2", 
        "\[RightDoubleBracket]"}]}]], "/.", "p0"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.527518330023939*^9, 3.527518359221435*^9}, {
  3.52751847534053*^9, 3.527518533884234*^9}, {3.527679025037657*^9, 
  3.52767904315482*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["linearized stance-COM slope", "Subsubsection",
 CellChangeTimes->{{3.527518559834362*^9, 3.527518563409494*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"slopeStCOML", "=", 
   RowBox[{"Linearize", "[", 
    RowBox[{"slopeStCOM", ",", 
     RowBox[{"Flatten", "[", "q", "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{"0", ",", 
       RowBox[{"{", "5", "}"}]}], "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5276173618152375`*^9, 3.5276173838424973`*^9}, {
  3.527679058261908*^9, 3.527679064807281*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Non-Stance Leg", "Subsection",
 CellChangeTimes->{{3.520696369485465*^9, 3.520696373189542*^9}, {
  3.527518724466259*^9, 3.527518730429883*^9}, {3.52754059614551*^9, 
  3.527540598788119*^9}}],

Cell[CellGroupData[{

Cell["non-stance slope", "Subsubsection",
 CellChangeTimes->{{3.520696408621931*^9, 3.5206964120770273`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"nsslope", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.520696380896888*^9, 3.5206963990299892`*^9}, {
   3.520696437432332*^9, 3.520696439262645*^9}, {3.520709844151616*^9, 
   3.520709845340612*^9}, {3.527517752506807*^9, 3.527517756889133*^9}, 
   3.527518743463629*^9, 3.527617552710156*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["linearized non-stance slope", "Subsubsection",
 CellChangeTimes->{{3.520696416053858*^9, 3.5206964219347477`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"nsslopeL", "=", 
   RowBox[{"Linearize", "[", 
    RowBox[{"nsslope", ",", 
     RowBox[{"Flatten", "[", "q", "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{"0", ",", 
       RowBox[{"{", "5", "}"}]}], "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5276174796189756`*^9, 3.5276175122438416`*^9}, {
  3.527679109902063*^9, 3.527679111479476*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["hip angle", "Subsubsection",
 CellChangeTimes->{{3.520696574436529*^9, 3.52069657648393*^9}, {
  3.527518807049567*^9, 3.527518808451294*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"HipAngle", " ", "=", " ", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";"}]], "Input",
 CellChangeTimes->{{3.52069657822197*^9, 3.5206965953274107`*^9}, {
  3.527518813458147*^9, 3.52751882174494*^9}, {3.5276791206520987`*^9, 
  3.527679122172039*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["stance-COM slope", "Subsubsection",
 CellChangeTimes->{{3.527518321277124*^9, 3.527518327763413*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"slopeNStCOM", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        SubscriptBox["p", "COM"], "\[LeftDoubleBracket]", "1", 
        "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        SubscriptBox["p", "COM"], "\[LeftDoubleBracket]", "2", 
        "\[RightDoubleBracket]"}]}]], "/.", "p0"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.527518330023939*^9, 3.527518359221435*^9}, {
  3.52751847534053*^9, 3.527518533884234*^9}, {3.527679025037657*^9, 
  3.52767904315482*^9}, {3.527679164292862*^9, 3.5276791786973877`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["linearized stance-COM slope", "Subsubsection",
 CellChangeTimes->{{3.527518559834362*^9, 3.527518563409494*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"slopeNStCOML", "=", 
   RowBox[{"Linearize", "[", 
    RowBox[{"slopeNStCOM", ",", 
     RowBox[{"Flatten", "[", "q", "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{"0", ",", 
       RowBox[{"{", "5", "}"}]}], "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5276173618152375`*^9, 3.5276173838424973`*^9}, {
  3.527679058261908*^9, 3.527679064807281*^9}, {3.5276791819494867`*^9, 
  3.527679184599461*^9}}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Hip Position", "Subsection",
 CellChangeTimes->{{3.5206971864248543`*^9, 3.520697188943389*^9}}],

Cell[CellGroupData[{

Cell["Hip Position", "Subsubsection",
 CellChangeTimes->{{3.520697193656226*^9, 3.520697196463649*^9}, {
  3.527518857042669*^9, 3.527518859540667*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"HipPos", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5206971983837137`*^9, 3.5206972146818438`*^9}, {
   3.527518863691704*^9, 3.5275188686553*^9}, 3.527519025998588*^9, 
   3.5276175485269165`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Linearized Hip Position", "Subsubsection",
 CellChangeTimes->{{3.52069721892881*^9, 3.520697223591626*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"HipPosL", "=", 
   RowBox[{"Linearize", "[", 
    RowBox[{"HipPos", ",", 
     RowBox[{"Flatten", "[", "q", "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{"0", ",", 
       RowBox[{"{", "5", "}"}]}], "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5276175262196407`*^9, 3.5276175561613536`*^9}, {
  3.527679141991006*^9, 3.5276791437911386`*^9}}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Write Outputs To disk", "Section",
 CellChangeTimes->{{3.527176889623287*^9, 3.5271768951406612`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<stCOMFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"slopeStCOM", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltastCOMFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"slopeStCOML", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<nstCOMFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"slopeNStCOM", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltanstCOMFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"slopeNStCOML", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<hipposFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"HipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaHipposFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"HipPosL", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<nsslopeFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"nsslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaNSslopeFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"nsslopeL", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.52717692204714*^9, 3.5271769389718723`*^9}, {
   3.527177770383316*^9, 3.5271777843116817`*^9}, 3.5273698919196157`*^9, {
   3.5275347337078667`*^9, 3.527534858604323*^9}, {3.5276793326637297`*^9, 
   3.527679398682186*^9}}],

Cell[CellGroupData[{

Cell["PERL", "Subsection",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}}],

Cell["this next line assumes that perl is in your path", "Text",
 CellChangeTimes->{{3.513015230408543*^9, 3.513015241200741*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2mat_output.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.504476276692404*^9, 3.504476301348216*^9}, {
   3.504478868125564*^9, 3.504478869967337*^9}, {3.504538702237328*^9, 
   3.504538704315194*^9}, {3.504631487786531*^9, 3.504631500259924*^9}, {
   3.504632214564033*^9, 3.504632215194798*^9}, {3.504635416937057*^9, 
   3.50463541912094*^9}, {3.504874639115547*^9, 3.504874639679282*^9}, 
   3.504876166859266*^9, {3.505578303460942*^9, 3.5055783039289427`*^9}, {
   3.512922890945364*^9, 3.512922891900902*^9}, {3.5129244660544987`*^9, 
   3.51292449464812*^9}, {3.512925468730114*^9, 3.512925469241001*^9}, {
   3.513011661780643*^9, 3.513011661926162*^9}, {3.51301497676734*^9, 
   3.513015001300451*^9}, {3.5158700422275343`*^9, 3.515870043613613*^9}, {
   3.515870412159693*^9, 3.5158704163519325`*^9}, {3.5158863672774124`*^9, 
   3.5158863722382207`*^9}, {3.5271773836362467`*^9, 3.527177386040779*^9}}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5155188337118206`*^9, 3.5155213252703295`*^9, {3.515540878198961*^9, 
   3.515540903604343*^9}, 3.5158407423966546`*^9, 3.5158416340670977`*^9, 
   3.5158498815118246`*^9, 3.515850149364145*^9, 3.5158505826739287`*^9, 
   3.5158508174343567`*^9, 3.5158510975593786`*^9, 3.515851653032055*^9, 
   3.5158525885305624`*^9, 3.5158527509368515`*^9, 3.5158527931192636`*^9, 
   3.51585495715904*^9, 3.5158550282321053`*^9, 3.5158552191920276`*^9, 
   3.515855253120968*^9, 3.515865262254135*^9, 3.515865638210639*^9, 
   3.51586592793721*^9, 3.51587048063961*^9, 3.5158730153755884`*^9, 
   3.5158739792467184`*^9, 3.515874203634553*^9, 3.515886694531988*^9, 
   3.5158874913514147`*^9, 3.515889070384316*^9, 3.5158900577352266`*^9, 
   3.515890252205177*^9, 3.5159482968094587`*^9, 3.515948944374403*^9, 
   3.515953395315056*^9, 3.515953710496083*^9, 3.5159537761788397`*^9, 
   3.5159726585821743`*^9, 3.5159857720472217`*^9, 3.516707085463479*^9, 
   3.516709802924909*^9, 3.5167112898219547`*^9, 3.5167126251093287`*^9, 
   3.516731910384804*^9, 3.51675156425562*^9, 3.517679232673436*^9, 
   3.517679752897191*^9, 3.517680087541332*^9, 3.5176859974273577`*^9, 
   3.5176980466975365`*^9, 3.5176984843215675`*^9, 3.517698597905064*^9, 
   3.520109553737687*^9, 3.520120022112444*^9, 3.520177851995036*^9, 
   3.5201861307705555`*^9, 3.520256997752266*^9, 3.5202590894559045`*^9, 
   3.520339215071435*^9, 3.5204373594555745`*^9, 3.5204379012015605`*^9, 
   3.5204436639822655`*^9, 3.520449216190834*^9, 3.520456964627019*^9, 
   3.520532857654432*^9, 3.5205334050077386`*^9, 3.520533641289253*^9, 
   3.520700208631287*^9, 3.520713025182435*^9, 3.520715993050187*^9, 
   3.520716372698902*^9, 3.5207164419848647`*^9, 3.520720498096861*^9, 
   3.520734731372958*^9, 3.5207350240366974`*^9, 3.520735233941703*^9, 
   3.5207876835786457`*^9, 3.520788877843954*^9, 3.5208972402433395`*^9, 
   3.5213037296170855`*^9, 3.5213038482928734`*^9, 3.52148604821651*^9, 
   3.521487916633377*^9, 3.5222451196790495`*^9, 3.522246794946869*^9, 
   3.52224990060228*^9, 3.522330715643431*^9, 3.5223566127951155`*^9, 
   3.5223569178824873`*^9, 3.5224296564487324`*^9, 3.522510361860586*^9, 
   3.5225118733730397`*^9, 3.522708144818318*^9, 3.5227751575219574`*^9, 
   3.522776845738859*^9, 3.5230285840646763`*^9, 3.5230306390121126`*^9, 
   3.523031339818296*^9, 3.5235572703774643`*^9, 3.52372487579274*^9, 
   3.5237265671694813`*^9, 3.5239837459472656`*^9, 3.5239849930135937`*^9, 
   3.5239893079543943`*^9, 3.5239903317719536`*^9, 3.523992025216813*^9, 
   3.5239921368791995`*^9, 3.5239948186215863`*^9, 3.5239949284198666`*^9, 
   3.5239950392682066`*^9, 3.5242481412858143`*^9, 3.524248528452959*^9, 
   3.5242499977068315`*^9, 3.5244001128388147`*^9, 3.5244002976151495`*^9, 
   3.5244062695321836`*^9, 3.524406550370155*^9, 3.5244067850253863`*^9, 
   3.524416267410578*^9, 3.524417771154109*^9, 3.5244178686872463`*^9, 
   3.5244183936479816`*^9, 3.524418700642165*^9, 3.524422224071623*^9, 
   3.5244223112816105`*^9, 3.524422752104824*^9, 3.5244238419211583`*^9, 
   3.524442216849144*^9, 3.524442742387203*^9, 3.5244438493725195`*^9, 
   3.5244456864925966`*^9, 3.5244458645247793`*^9, 3.5244468702188396`*^9, 
   3.5245024335606823`*^9, 3.5245119471381826`*^9, 3.5245883990742893`*^9, 
   3.525090287048571*^9, 3.525091695188668*^9, 3.5250919519797196`*^9, 
   3.5250921900848627`*^9, 3.5250924453299904`*^9, 3.525092571060454*^9, 
   3.52509268956332*^9, 3.5250928935899105`*^9, 3.5250932069259415`*^9, 
   3.525098766927955*^9, 3.5254479897919855`*^9, 3.525461216302328*^9, 
   3.5255182690806684`*^9, 3.525824548871134*^9, 3.527177429714081*^9, 
   3.527177710324808*^9, 3.5271838004534616`*^9, 3.5273332498298492`*^9, 
   3.527334075596567*^9, 3.527451043872353*^9, 3.5275349707840014`*^9, 
   3.527535012201075*^9, 3.527617750656283*^9, 3.5276795461969337`*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}, Open  ]]
},
WindowSize->{959, 1026},
WindowMargins->{{-9, Automatic}, {Automatic, -8}},
ShowSelection->True,
Magnification->1.100000023841858,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[567, 22, 336, 5, 91, "Title"],
Cell[CellGroupData[{
Cell[928, 31, 196, 3, 78, "Section"],
Cell[1127, 36, 768, 16, 89, "Input"],
Cell[1898, 54, 173, 4, 31, "Input"],
Cell[2074, 60, 1165, 34, 89, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[3276, 99, 154, 2, 78, "Section"],
Cell[CellGroupData[{
Cell[3455, 105, 7198, 213, 659, "Input"],
Cell[10656, 320, 858, 20, 51, "Output"],
Cell[11517, 342, 1136, 31, 92, "Output"],
Cell[12656, 375, 1137, 31, 101, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[13830, 411, 135, 2, 44, "Subsection"],
Cell[13968, 415, 1263, 39, 118, "Input"],
Cell[CellGroupData[{
Cell[15256, 458, 148, 2, 31, "Subsubsection"],
Cell[15407, 462, 153, 2, 33, "Text"],
Cell[15563, 466, 5128, 124, 181, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[20728, 595, 134, 2, 31, "Subsubsection"],
Cell[20865, 599, 4686, 110, 359, "Input"],
Cell[25554, 711, 2032, 52, 158, "Input"],
Cell[27589, 765, 1709, 42, 132, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[29335, 812, 203, 3, 31, "Subsubsection"],
Cell[29541, 817, 2825, 79, 139, "Input"],
Cell[32369, 898, 5403, 152, 181, "Input"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[37833, 1057, 158, 2, 85, "Section"],
Cell[37994, 1061, 157, 2, 33, "Text"],
Cell[38154, 1065, 921, 25, 69, "Input"],
Cell[39078, 1092, 579, 12, 67, "Input"],
Cell[CellGroupData[{
Cell[39682, 1108, 107, 1, 31, "Subsubsection"],
Cell[39792, 1111, 973, 26, 62, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[40802, 1142, 118, 1, 31, "Subsubsection"],
Cell[40923, 1145, 404, 10, 34, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[41364, 1160, 199, 3, 44, "Subsection"],
Cell[CellGroupData[{
Cell[41588, 1167, 109, 1, 31, "Subsubsection"],
Cell[41700, 1170, 1239, 32, 62, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[42976, 1207, 120, 1, 31, "Subsubsection"],
Cell[43099, 1210, 398, 10, 34, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[43534, 1225, 148, 2, 31, "Subsubsection"],
Cell[43685, 1229, 398, 10, 34, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[44120, 1244, 107, 1, 31, "Subsubsection"],
Cell[44230, 1247, 1022, 26, 62, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[45289, 1278, 118, 1, 31, "Subsubsection"],
Cell[45410, 1281, 457, 11, 34, "Input"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[45916, 1298, 102, 1, 44, "Subsection"],
Cell[CellGroupData[{
Cell[46043, 1303, 152, 2, 31, "Subsubsection"],
Cell[46198, 1307, 510, 13, 34, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[46745, 1325, 113, 1, 31, "Subsubsection"],
Cell[46861, 1328, 398, 10, 34, "Input"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[47320, 1345, 108, 1, 85, "Section"],
Cell[47431, 1348, 4020, 116, 683, "Input"],
Cell[CellGroupData[{
Cell[51476, 1468, 91, 1, 44, "Subsection"],
Cell[51570, 1471, 130, 1, 33, "Text"],
Cell[CellGroupData[{
Cell[51725, 1476, 1269, 23, 77, "Input"],
Cell[52997, 1501, 3947, 55, 34, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}
]
*)

(* End of internal cache information *)
